/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

/**
* How many floors there is now
**/
#define NUM_OF_FLOORS 3
/**
* The height of the second floor from the first floor
**/
#define FLOOR_HEIGHT_1 4.2
#define FLOOR_HEIGHT_2 (FLOOR_HEIGHT_1+3.7)


#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"
#include "JPEGEncoder.hpp"  ///< For saving JPEG
/// For reading the data from file

void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
	CJPEGEncoder encoder;
	encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
	FILE *f = fopen(fname,"wb");
	fwrite(encoder.GetJPEGDataPtr(), 
				 encoder.GetJPEGDataSize(), 
																 1, f);
	fclose(f);
		
}

void readImuWalkOdo2D(FILE *f,mcl::scan &meas, mcl::pose &p){
		meas.N=0;
		float roll,pitch,z;
		fscanf(f,"%f %f %f %f %f %f\n",&p.x,&p.y,&z,&roll,&pitch, &p.a); ///< Pose	
		//p.a = -p.a;	
		p.y = -p.y;
}


void readImuWalkOdo3D(FILE *f,mcl::scan &meas, mcl::pose &p, float &tset){
		meas.N=0;
		float roll,pitch;
		fscanf(f,"%f %f %f %f %f %f\n",&p.x,&p.y,&tset,&roll,&pitch, &p.a); ///< Pose	
		//p.a = -p.a;	
		p.y = -p.y;
		tset = -tset+FLOOR_HEIGHT_1; ///< odometry is calculated with respect to second floor
}


int readScanLine181(FILE *f,mcl::scan &meas, mcl::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int readSilmuPoseData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %d\n",&t1,&p.x,&p.y,&t2,&p.a,&i); ///< Pose
		meas.N = 0;
		
		return 0;
}

 int readSilmuPoseDataNuppu(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2,foo1,foo2;
		int i;
		char c;
		//fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&p.x,&p.y,&t2,&p.a,&i,&foo1,&foo2); ///< Pose
		fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&foo1,&foo2,&t2,&p.a,&i,&p.x,&p.y); ///< Pose
		meas.N = 0;
		
		return 0;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}

void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
vector<mcl::pose> godo; ///< Will be the corrected odometry
vector<mcl::pose> odo;  ///< Original odometry
static int lastCorrected=-1;
void odoCorrector(mcl::pose &p, int index, float calib){
		mcl::pose mest;
		mcl::pose corPos;
		
		if(lastCorrected == -1){
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						mest.x *=calib;mest.y*=calib;
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				for(int i=index;i>0;i--){
						mest.setToDifferentialPose( odo[i-1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i-1] = corPos;
				}	
		}
		else{
				mcl::pose orig = godo[index];
				mcl::pose err = orig;
				int N = index-lastCorrected;
				int last = lastCorrected;
						
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				
				if(err.a > M_PI){
						
						//fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a>M_PI) err.a-=2.0*M_PI; 
				}else if(err.a < -M_PI){
						//fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a < -M_PI) err.a+=2.0*M_PI; 
				}
				
				fprintf(stderr,"Orig_err (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.a/=(float)(N);
				
				
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				///Now compute the correction from the last correctio to present
				/// First the heading
				godo[index] = orig;
				for(int i=last;i<index;i++){
						mest.setToDifferentialPose( godo[i+1],godo[i]);
						if(i>last) godo[i] = corPos; 
						mest.a -= err.a;
						corPos = godo[i].integrateDifferential(mest);
				}
				godo[index]=corPos;
			
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				//fprintf(stderr,"Err_Now (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.y/=(float)(N);
				err.x/=(float)(N);
				
				for(int i=last;i<=index;i++){
						godo[i].x -= (i-last)*err.x;
						godo[i].y -= (i-last)*err.y;
				}
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
		}
}
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
void estimateHeading(vector<mcl::pose> &odo){
		float heading = 0;
		float head_old = 0;
		float da;
		vector<mcl::pose> tmp;
		tmp=odo;
		for(int i=10;i<odo.size();i+=1){
			da = (odo[i].a-odo[i-10].a);
			//heading =  0.3 *atan2( (odo[i].y-odo[i-10].y),(odo[i].x-odo[i-10].x) ) + 0.7*(da+head_old);
			heading =  0.95 *atan2( (odo[i].y-odo[i-10].y),(odo[i].x-odo[i-10].x) ) + 0.05*(da+head_old);
			tmp[i].a=heading;
			head_old = heading;
		}
		odo=tmp;
}

#define FLOOR_DISTANCE_TH 1.0
bool isBetweenFloors(float zest){
		float mindist;
		bool ret;
		
		if(fabs(zest)<FLOOR_DISTANCE_TH) return false;
		if(fabs(zest-FLOOR_HEIGHT_1)<FLOOR_DISTANCE_TH) return false;
		if(fabs(zest-FLOOR_HEIGHT_2)<FLOOR_DISTANCE_TH) return false;
		return true;
}

float getFloorHeight(float zest){
		if(fabs(zest)<FLOOR_DISTANCE_TH){
				return 0.0;
		}
		if(fabs(zest-FLOOR_HEIGHT_1)<FLOOR_DISTANCE_TH){
				return FLOOR_HEIGHT_1;
		}
		if(fabs(zest-FLOOR_HEIGHT_2)<FLOOR_DISTANCE_TH){
				return FLOOR_HEIGHT_2;
		}
		
		fprintf(stderr,"Hmmm.... should not be here");
		return 0.0;
}

int getFloorNumber(float zest){
		if(fabs(zest)<FLOOR_DISTANCE_TH)return 0;
		if(fabs(zest-FLOOR_HEIGHT_1)<FLOOR_DISTANCE_TH)return 1;
		if(fabs(zest-FLOOR_HEIGHT_2)<FLOOR_DISTANCE_TH)return 2;
		fprintf(stderr,"Hmmm.... should not be here");
		return 0;
}

int main(int argc, char *argv[]){
		
		if(argc != 2){
				fprintf(stderr,"\nUsage: ./topo [logfile]\n\n");
				fprintf(stderr,"[logfile] - IMU Log file (format [x y z r p y\n])\n");
				return 0;
		}
		
		char *logfile=argv[1];
				
		FILE *log = fopen(logfile,"rt");
		if(log==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
	
		CTopoOccuMCL *topo;
		
		CTopoOccuMCL *second_floor = new CTopoOccuMCL();
		CTopoOccuMCL *first_floor = new CTopoOccuMCL();
		CTopoOccuMCL *third_floor = new CTopoOccuMCL();
		float z_est = 0;         ///< estimate of z
		int floor = 1;           ///< which floor
		bool isInStairs=false;   ///< Is it in stairs
		
		second_floor->initialize("../../res/autmap-koko.txt",
														 1.0, 
														 10000, 
														 mcl::pose(4,1,165*M_PI/180),  
														 mcl::pose(1.0,1.0,30*M_PI/180),
														 0.15,
														 1.0);
		
		first_floor->initialize("../../res/tuas1k.txt",
														1.0, 
														10000, 
														mcl::pose(4,1,165*M_PI/180),  
														mcl::pose(1.0,1.0,30*M_PI/180),
														0.15,
														1.0);
		third_floor->initialize("../../res/tuas3k.txt",
														1.0, 
														10000, 
														mcl::pose(4,1,165*M_PI/180),  
														mcl::pose(1.0,1.0,30*M_PI/180),
														0.15,
														1.0);
		
		//second_floor->O->loadLineMap("../line/aula.txt", 1.0,0,false,false,0);
		
		int Nx[NUM_OF_FLOORS],Ny[NUM_OF_FLOORS];
		unsigned char *rgb[NUM_OF_FLOORS];
		first_floor->O->getDim(Nx[0],Ny[0]);
		second_floor->O->getDim(Nx[1],Ny[1]);
		third_floor->O->getDim(Nx[2],Ny[2]);
		///Initialize Image display
		for(int i=0;i<NUM_OF_FLOORS;i++){
				rgb[i] = (unsigned char *) malloc(3*Nx[i]*Ny[i]);
				addImageWindow(Nx[i], Ny[i]);
		}
		glutImageGUI_Start();
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;
		vector<mcl::pose> odometry;
		vector<float> z_odo;
		float zeta;
		while(!feof(log)){
				readImuWalkOdo3D(log,s,odo_ref,zeta);
				odometry.push_back(odo_ref);	
				z_odo.push_back(zeta);
		}
		fclose(log);

		estimateHeading(odometry);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		
		odo=odometry;
		godo = odo;
		mcl::pose initpose(4,1,165*M_PI/180);
		odoCorrector(initpose, 0,1.0);
		odometry=godo;

		vector<mcl::pose> path;
		
		odo_ref = odometry[0];
		
		float dz_filt=0; ///< filtered height
		float z_cur=z_odo[0], z_old=z_odo[0];
		z_est = z_odo[0];
		
		FILE *ftrajout = fopen("trajout.txt","wt");
		
		for(int odo_cnt=1;odo_cnt<odometry.size();odo_cnt++){
				odo_cur = odometry[odo_cnt];
				z_cur = z_odo[odo_cnt];
				////////////////////////////////////////////////////////////////////////////
				/// The Z-estimation
				////////////////////////////////////////////////////////////////////////////
				dz_filt = 0.6 * dz_filt + 0.4 * (z_cur-z_old);
				
				isInStairs = false;
				if(fabs(dz_filt)>0.05 || isBetweenFloors(z_est)){
						fprintf(stderr,"In stairs: ");
						isInStairs=true;
				}
				 
				if(isInStairs){
						z_est+=(z_cur-z_old);
				}else{
						float error = getFloorHeight(z_est) - z_est;
						z_est += 0.4*error; 
				}
				
				
				fprintf(stderr,"Z=%.2f\n",z_est);
				if(floor != getFloorNumber(z_est) && !isInStairs){
						fprintf(stderr,"####### FLOOR CHANGED TO %d\n",getFloorNumber(z_est));
						CTopoOccuMCL *from,*to;
						
						
						if(floor==0) from = first_floor;
						else if(floor==1) from = second_floor;
						else if(floor==2) from = third_floor;
						
						if(getFloorNumber(z_est)==0) to=first_floor;
						else if(getFloorNumber(z_est)==1) to=second_floor;
						else if(getFloorNumber(z_est)==2) to=third_floor;
									
						for(int i=0;i<from->filt.NumOfParticles;i++){
								to->filt.Particles[i] = from->filt.Particles[i];
						}
						floor = getFloorNumber(z_est);
				} 
				
				z_old = z_cur;
				////////////////////////////////////////////////////////////////////////////
				/// The MCL
				////////////////////////////////////////////////////////////////////////////
				mest.setToDifferentialPose( odo_cur,odo_ref);
				float calib=1.0;
				mest.x *= calib;
				mest.y *= calib;
				
				switch(floor){
						case 0:
								topo=first_floor;
								break;
						case 1:
								topo = second_floor;
								break;
						case 2:
								topo = third_floor;
								break;
						default:
								fprintf(stderr,"Invalid Floor - exit");
								exit(1);
								break;
				}
				
				
				//topo.runMCL(mest, mcl::pose(fabs(mest.x)*0.02+0.01,fabs(mest.y)*0.02+0.01,0.005*fabs(mest.a)),  s);
				topo->runMCL(mest, mcl::pose(fabs(mest.x)*0.2,fabs(mest.y)*0.2,0.004*fabs(mest.a)),  s);
				odo_ref = odo_cur;
				
				mcl::pose mp = topo->filt.getDistributionMean(true); ///< The distribution mean
				mcl::pose var = topo->filt.getDistributionVariances();
				mp.to2PI();
				if(sqrt(var.x*var.x+var.y*var.y)<35.0)	path.push_back(mp);
				if(cnt%1==0){
						fprintf(stderr,"Pos:(%.2f,%.2f,%.2f) ",mp.x,mp.y,mp.a);
						//fprintf(stderr,"Est:(%.2f,%.2f,%.2f) ",mest.x,mest.y,mest.a);
						//fprintf(stderr,"VAR: %.2f  %.2f %.2f\n",var.x,var.y,var.a);
				}
				if(!isInStairs) fprintf(ftrajout,"%f %f %f %d\n",mp.x,mp.y,z_est,floor);
				else fprintf(ftrajout,"%f %f %f %d\n",mp.x,mp.y,z_est,3);
				fflush(ftrajout);
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				
				if(cnt%100==0){
					char name[59];
					sprintf(name,"%5d_global_init.jpg",cnt);
					saveJPEG(name, rgb[floor],Nx[floor], Ny[floor]);
				}
				
				if(cnt%2==0){
				setRGBmap(rgb[floor],topo->O, 190,190, 240);
					Grid::TCoord2D coord;
					Grid::TPixel pix;
					for(int i=0;i<topo->filt.NumOfParticles;i++){
						coord.x = topo->filt.Particles[i].x;
						coord.y = topo->filt.Particles[i].y;
						pix = topo->O->toPixel(coord);
						if(cnt == 0) setRGBpixBold(rgb[floor],pix.x,pix.y, Nx[floor], 255,255, 0);
						else setRGBpix(rgb[floor],pix.x,pix.y, Nx[floor], 255,255, 0);
					}
					
					Grid::TCoord2D ocoord;
					ocoord.x = mp.x;ocoord.y = mp.y;
					pix = topo->O->toPixel(ocoord);
					drawLine(rgb[floor], Nx[floor],Ny[floor], topo->O->getResolution(),pix.x,pix.y,mp.a, 2.0,255,0,0);
					drawLine(rgb[floor], Nx[floor],Ny[floor], topo->O->getResolution(),pix.x,pix.y,mp.a+M_PI/2, 2.0,0,255,0);
					
					for(int i=0;i<path.size();i+=1){
							ocoord.x = path[i].x;ocoord.y = path[i].y;
							pix = topo->O->toPixel(ocoord);
							setRGBpixBold(rgb[floor],pix.x,pix.y, Nx[floor], 255,0, 0 );
					}
					
					
					if(cnt == 0){
						for(int i=0;i<odometry.size();i+=1){
								ocoord.x = odometry[i].x;ocoord.y = odometry[i].y;
								pix = topo->O->toPixel(ocoord);
								
								setRGBpixBold(rgb[floor],pix.x,pix.y, Nx[floor], 255,0, 0 );
								//drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,odometry[i].a, 2.0,255,0,0);
								//drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,odometry[i].a+M_PI/2, 2.0,0,255,0);
								//if(i%1==0) {displayImage(0, rgb,Ny,Nx); usleep(100000);}
						}
					}
					
					displayImage(floor, rgb[floor],Ny[floor],Nx[floor]);
					if(cnt == 0) {
							saveJPEG("0_global_init.jpg", rgb[floor],Nx[floor], Ny[floor]);
							char c;
							fprintf(stderr,"PRESS ENTER TO CONTINUE\n");
							fscanf(stdin,"%c",&c);
					}
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////
				
				cnt++;
		}
		fclose(ftrajout);
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		
		fprintf(stderr,"The End - press ctrl C to stop\n");
		while(1){
				setRGBmap(rgb[floor],topo->O, 200,255, 200);
			Grid::TCoord2D coord;
			Grid::TPixel pix;
			
			for(int i=0;i<topo->filt.NumOfParticles;i++){
				coord.x = topo->filt.Particles[i].x;
				coord.y = topo->filt.Particles[i].y;
				pix = topo->O->toPixel(coord);
				setRGBpix(rgb[floor],pix.x,pix.y, Nx[floor], 255,255, 0);
			}
					
			Grid::TCoord2D ocoord;
			
			for(int i=0;i<path.size();i+=1){
				ocoord.x = path[i].x;ocoord.y = path[i].y;
				pix = topo->O->toPixel(ocoord);
				setRGBpixBold(rgb[floor],pix.x,pix.y, Nx[floor], 255,0, 0 );
			}
			displayImage(floor, rgb[floor],Ny[floor],Nx[floor]);
			usleep(1000*1000);
		}
		return 0;
}